#include <cmath>

#include "lshape2.h"

namespace perception
{
    namespace algorithm
    {

        LShape2::LShape2()
        {
        }
        LShape2::~LShape2()
        {
        }

        float LShape2::Compute(std::vector<data_cell> &cells)
        {
            float best_rad;
            float rad = 0;
            float best_cost = -10000;
            float cost;
            for (int i = 0; i < 90 / th_rad; i++)
            {
                auto tmp_cell = cells;
                // std::cout << "tmp_cell0:" <<  tmp_cell[0].x << std::endl;
                // 转换坐标
                float rad_t = rad * deg2rad;
                CoordinateTransformation(tmp_cell, rad_t);
                // 损失
                switch (cost_type)
                {
                case Cost::AREA:
                    cost = CalcAreaCriterion(tmp_cell); // 最小面积损失
                    break;
                case Cost::CLOSENESS:
                    cost = CalcClosenessCriterion(tmp_cell); // 最贴近准则的损失
                    break;
                case Cost::VARIANCE:
                    cost = CalcVarianceCriterion(tmp_cell); // 方差损失
                    break;

                default:
                    break;
                }
                if (best_cost < cost)
                {
                    // std::cout << "cost:" << cost << " rad:" << rad <<std::endl;
                    best_cost = cost;
                    best_rad = rad;
                }
                rad = rad + th_rad;
            }
            return best_rad * deg2rad;
        }
        float LShape2::Compute(XYZI &points)
        {
            float best_rad;
            float rad = 0;
            float best_cost = -10000;
            float cost;
            for (int i = 0; i < 90 / th_rad; i++)
            {
                auto tmp_cell = points;
                // std::cout << "tmp_cell0:" <<  tmp_cell[0].x << std::endl;
                // 转换坐标
                float rad_t = rad * deg2rad;
                CoordinateTransformation(tmp_cell, rad_t);
                // 损失
                switch (cost_type)
                {
                case Cost::AREA:
                    cost = CalcAreaCriterion(tmp_cell); // 最小面积损失
                    break;
                case Cost::CLOSENESS:
                    cost = CalcClosenessCriterion(tmp_cell); // 最贴近准则的损失
                    break;
                case Cost::VARIANCE:
                    cost = CalcVarianceCriterion(tmp_cell); // 方差损失
                    break;

                default:
                    break;
                }
                if (best_cost < cost)
                {
                    // std::cout << "cost:" << cost << " rad:" << rad <<std::endl;
                    best_cost = cost;
                    best_rad = rad;
                }
                rad = rad + th_rad;
            }
            return best_rad * deg2rad;
        }
        void LShape2::CoordinateTransformation(std::vector<data_cell> &cells, float &rad)
        {
            auto tmp = cells;
            for (size_t i = 0; i < cells.size(); i++)
            {
                cells[i].x = tmp[i].x * std::cos(rad) + tmp[i].y * std::sin(rad);
                cells[i].y = tmp[i].y * std::cos(rad) - tmp[i].x * std::sin(rad);
            }
        }
        void LShape2::CoordinateTransformation(XYZI &cells, float &rad)
        {
            auto tmp = cells;
            for (size_t i = 0; i < cells.points.size(); i++)
            {
                cells.points[i].x = tmp.points[i].x * std::cos(rad) + tmp.points[i].y * std::sin(rad);
                cells.points[i].y = tmp.points[i].y * std::cos(rad) - tmp.points[i].x * std::sin(rad);
            }
        }
        float LShape2::CalcAreaCriterion(std::vector<data_cell> &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            cost = -(max_min[1] - max_min[0]) * (max_min[3] - max_min[2]);
            return cost;
        }
        float LShape2::CalcAreaCriterion(XYZI &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            cost = -(max_min[1] - max_min[0]) * (max_min[3] - max_min[2]);
            return cost;
        }
        float LShape2::CalcClosenessCriterion(std::vector<data_cell> &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            // 求最小值
            for (auto &cell : cells)
            {
                auto x_min = std::min(std::fabs(max_min[0] - cell.x), std::fabs(max_min[1] - cell.x));
                auto y_min = std::min(std::fabs(max_min[2] - cell.y), std::fabs(max_min[3] - cell.y));
                auto t = std::max(std::min(x_min, y_min), min_closter);
                cost = cost + 1.0 / t;
            }
            return cost;
        }
        float LShape2::CalcClosenessCriterion(XYZI &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            // 求最小值
            for (auto &cell : cells.points)
            {
                auto x_min = std::min(std::fabs(max_min[0] - cell.x), std::fabs(max_min[1] - cell.x));
                auto y_min = std::min(std::fabs(max_min[2] - cell.y), std::fabs(max_min[3] - cell.y));
                if (max_closter < std::min(x_min * x_min, y_min * y_min))
                    continue;
                auto t = std::max(std::min(x_min * x_min, y_min * y_min), min_closter);
                cost = cost + 1.0 / t;
            }
            return cost;
        }
        float LShape2::CalcVarianceCriterion(std::vector<data_cell> &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            std::vector<float> E1;
            std::vector<float> E2;
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            for (auto &cell : cells)
            {
                auto x_min = std::min(std::fabs(max_min[0] - cell.x), std::fabs(max_min[1] - cell.x));
                auto y_min = std::min(std::fabs(max_min[2] - cell.y), std::fabs(max_min[3] - cell.y));
                if (x_min < y_min)
                    E1.emplace_back(x_min);
                else
                    E2.emplace_back(y_min);
            }
            float E1_sum = 0;
            float E1_mean = 0;
            float E2_sum = 0;
            float E2_mean = 0;
            float v1 = 0;
            float v2 = 0;
            if (!E1.empty())
            {
                E1_sum = std::accumulate(E1.begin(), E1.end(), 0.0);
                E1_mean = E1_sum / E1.size();
                for (int i = 0; i < E1.size(); i++)
                {
                    v1 = v1 + std::pow(E1[i] - E1_mean, 2);
                }
                v1 = v1 / E1.size();
            }
            if (!E2.empty())
            {
                E2_sum = std::accumulate(E2.begin(), E2.end(), 0.0);
                E2_mean = E2_sum / E2.size();
                for (int i = 0; i < E2.size(); i++)
                {
                    v2 = v2 + std::pow(E2[i] - E2_mean, 2);
                }
                v2 = v2 / E2.size();
            }
            cost = -v1 - v2;

            return cost;
        }
        float LShape2::CalcVarianceCriterion(XYZI &cells)
        {
            std::vector<float> max_min;
            max_min.resize(4);
            std::vector<float> E1;
            std::vector<float> E2;
            float cost = 0;
            ComputeMaxMin(cells, max_min);
            for (auto &cell : cells.points)
            {
                auto x_min = std::min(std::fabs(max_min[0] - cell.x), std::fabs(max_min[1] - cell.x));
                auto y_min = std::min(std::fabs(max_min[2] - cell.y), std::fabs(max_min[3] - cell.y));
                if (x_min < y_min)
                    E1.emplace_back(x_min);
                else
                    E2.emplace_back(y_min);
            }
            float E1_sum = 0;
            float E1_mean = 0;
            float E2_sum = 0;
            float E2_mean = 0;
            float v1 = 0;
            float v2 = 0;
            if (!E1.empty())
            {
                E1_sum = std::accumulate(E1.begin(), E1.end(), 0.0);
                E1_mean = E1_sum / E1.size();
                for (int i = 0; i < E1.size(); i++)
                {
                    v1 = v1 + std::pow(E1[i] - E1_mean, 2);
                }
                v1 = v1 / E1.size();
            }
            if (!E2.empty())
            {
                E2_sum = std::accumulate(E2.begin(), E2.end(), 0.0);
                E2_mean = E2_sum / E2.size();
                for (int i = 0; i < E2.size(); i++)
                {
                    v2 = v2 + std::pow(E2[i] - E2_mean, 2);
                }
                v2 = v2 / E2.size();
            }
            cost = -v1 - v2;

            return cost;
        }
        void LShape2::ComputeMaxMin(std::vector<data_cell> &cells, std::vector<float> &max_min)
        {
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            for (auto &cell : cells)
            {

                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z_min);
                max_z = std::max(max_z, cell.z_max);
            }
            max_min[0] = min_x;
            max_min[1] = max_x;
            max_min[2] = min_y;
            max_min[3] = max_y;
        }
        void LShape2::ComputeMaxMin(XYZI &cells, std::vector<float> &max_min)
        {
            float min_x = std::numeric_limits<float>::max();
            float min_z = std::numeric_limits<float>::max();
            float min_y = std::numeric_limits<float>::max();
            float max_x = -std::numeric_limits<float>::max();
            float max_y = -std::numeric_limits<float>::max();
            float max_z = -std::numeric_limits<float>::max();
            for (auto &cell : cells.points)
            {

                min_x = std::min(min_x, cell.x);
                max_x = std::max(max_x, cell.x);
                min_y = std::min(min_y, cell.y);
                max_y = std::max(max_y, cell.y);
                min_z = std::min(min_z, cell.z);
                max_z = std::max(max_z, cell.z);
            }
            max_min[0] = min_x;
            max_min[1] = max_x;
            max_min[2] = min_y;
            max_min[3] = max_y;
        }
    } // namespace perception
} // namespace avos